//===========================================================================
/*
  CS277 - Experimental Haptics
  Winter 2010, Stanford University

  You may use this program as a boilerplate for starting your homework
  assignments.  Use CMake (www.cmake.org) on the CMakeLists.txt file to
  generate project files for the development tool of your choice.  The
  CHAI3D library directory (chai3d-2.1.0) should be installed as a sibling
  directory to the one containing this project.

  These files are meant to be helpful should you encounter difficulties
  setting up a working CHAI3D project.  However, you are not required to
  use them for your homework -- you may start from anywhere you'd like.

  \author    Francois Conti & Sonny Chan
  \date      January 2010
*/
//===========================================================================

//---------------------------------------------------------------------------
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
//---------------------------------------------------------------------------
#include <GL/glew.h>
#include <cutil_inline.h> 
#include <cutil_math.h>

#include "SimulationForce.h"
#include "chai3d.h"
#include "paramgl.h"
#include "fluid.h"

//---------------------------------------------------------------------------
// DECLARED CONSTANTS
//---------------------------------------------------------------------------


int displayW  = 0;
int displayH  = 0;

// initial size (width/height) in pixels of the display window
const int WINDOW_SIZE_W         = 600;
const int WINDOW_SIZE_H         = 600;

// mouse menu options (right button)
const int OPTION_FULLSCREEN     = 1;
const int OPTION_WINDOWDISPLAY  = 2;

// maximum number of haptic devices supported in this demo
const int MAX_DEVICES           = 8;


//---------------------------------------------------------------------------
// DECLARED VARIABLES
//---------------------------------------------------------------------------


// width and height of the current window display

int window_id = -1;

cWorld* h_world;
cGeneric3dofPointer* h_tool;
SimulationForce* h_force_algorithm;
#define WORKSPACE_SCALE 32.f
cMesh* wall[6];

// a haptic device handler
cHapticDeviceHandler* handler;
// a pointer to the first haptic device detected on this computer
cGenericHapticDevice* hapticDevice = 0;
// labels to show haptic device position and update rate
double rateEstimate = 0;
#define SIM_SAMPLE_PERIOD 0.5f
cPrecisionClock simulation_clock;
int simulation_counter=0;
float simrateEstimate=0.f;

cPrecisionClock graphics_clock;
int graphics_counter=0;
float graphics_rateEstimate=0.f;
float graphics_time = 0.f;

pthread_mutex_t cuda_lock;


// material properties used to render the color of the cursors
cMaterial matCursorButtonON;
cMaterial matCursorButtonOFF;

// status of the main simulation haptics loop
bool simulationRunning = false;

// has exited haptics simulation thread
bool simulationFinished = false;

//do no display
bool benchmark = false;

#define VELOCITY_TRANSFER 0.0


float3 h_vel;
float3 h_pos;
float3 h_force;
int buttonStatus=0;
int preset = 0;

float force_decay = 0.9;
cVector3d force_accumulator;
float vel_decay = 0.5;
cVector3d vel_accumulator;

bool resetStuff = false;
bool resetVel = false;
float adaptiveDT = 0.f;

float tool_size = 1.f;
float h_mass=  2.f;



//display states
bool displayParam = false;
display_mode displayMode = DISPLAY_FLUID;
string displayModeText = "Fluid";
int buttonState = 0;
int ox, oy;
float cameraYaw = 00.0;
float cameraPitch = 0.0; 
ParamListGL *params;

float h_color_r =0.9f;
float h_color_g =0.8f;
float h_color_b =0.2f;
float injection_rate=2.0f;
float point_size=10.f;
float diff_rate =2.0f;
float stuff_diff_rate=0.1f;


//---------------------------------------------------------------------------
// DECLARED FUNCTIONS DECLARATION
//---------------------------------------------------------------------------

// callback when the window display is resized
void resizeWindow(int w, int h);

// callback when a keyboard key is pressed
void keySelect(unsigned char key, int x, int y);
void specialSelect(int key, int x, int y);

// callback when the right mouse button is pressed to select a menu item
void menuSelect(int value);

// function called before exiting the application
void close(void);

// main graphics callback
void updateGraphics(void);

// main haptics loop
void updateHaptics(void);

//main simulation loop
void updateSimulation(void);

//mouse
void mouse(int button, int state, int x, int y);

//mouse movement
void motion(int x, int y);

#include "HelpWindow.h"

void initGL(int argc, char* argv[]){
  //-----------------------------------------------------------------------
  // OPEN GL - WINDOW DISPLAY
  //-----------------------------------------------------------------------

  // initialize GLUT
  glutInit(&argc, argv);

  // retrieve the resolution of the computer display and estimate the position
  // of the GLUT window so that it is located at the center of the screen
  int screenW = glutGet(GLUT_SCREEN_WIDTH);
  int screenH = glutGet(GLUT_SCREEN_HEIGHT);
  int windowPosX = (screenW - WINDOW_SIZE_W) / 2;
  int windowPosY = (screenH - WINDOW_SIZE_H) / 2;

  // initialize the OpenGL GLUT window
  glutInitWindowPosition(windowPosX, windowPosY);
  glutInitWindowSize(WINDOW_SIZE_W, WINDOW_SIZE_H);
  glutInitDisplayMode(GLUT_RGB | GLUT_DEPTH | GLUT_DOUBLE);
  window_id = glutCreateWindow(argv[0]);
  glutDisplayFunc(updateGraphics);
  glutKeyboardFunc(keySelect);
  glutSpecialFunc(specialSelect);
  glutReshapeFunc(resizeWindow);
  glutMouseFunc(mouse);
  glutMotionFunc(motion);
  glutSetWindowTitle("Stable Fluid");

  

  glewInit();
  if (!glewIsSupported("GL_VERSION_2_0 GL_VERSION_1_5 GL_ARB_multitexture GL_ARB_vertex_buffer_object")) {
    fprintf(stderr, "Required OpenGL extensions missing.");
    exit(-1);
  }
  glEnable(GL_DEPTH_TEST);
  glClearColor(0.25, 0.25, 0.25, 1.0);

  glutReportErrors();

}


void initParams()
{
  // create a new parameter list
  params = new ParamListGL("fluid");
  params->SetFont(GLUT_BITMAP_8_BY_13, 13);
  params->AddParam(new Param<float>("Cursor Mass", h_mass, 1.0f, 4.0f, 1.f, &h_mass));
  //params->AddParam(new Param<float>("Cursor Size", tool_size, 1.0f, 3.0f, 0.5f, &tool_size));
  params->AddParam(new Param<float>("Cursor Red"  ,h_color_r, 0.0f, 1.0f, 0.01f, &h_color_r));
  params->AddParam(new Param<float>("Cursor Green",h_color_g, 0.0f, 1.0f, 0.01f, &h_color_g));
  params->AddParam(new Param<float>("Cursor Blue" ,h_color_b, 0.0f, 1.0f, 0.01f, &h_color_b));
  params->AddParam(new Param<float>("Inject Rate" ,injection_rate, 1.0f, 20.0f, 0.1f, &injection_rate));
  //params->AddParam(new Param<float>("Point Size" ,point_size, 1.0f, 40.0f, 0.1f, &point_size));

  params->AddParam(new Param<float>("Vel Diffusion", diff_rate, 0.0f, 20.0f, 0.1f, &diff_rate));
  params->AddParam(new Param<float>("Color Diffusion", stuff_diff_rate, 0.0f, 20.0f, 0.1f, &stuff_diff_rate));

  params->AddParam(new Param<float>("Force Smoothing", force_decay, 0.0f, 1.0f, 0.01f, &force_decay));
  params->AddParam(new Param<float>("Vel Smoothing", vel_decay, 0.0f, 1.0f, 0.01f, &vel_decay));
  
  
  

}

void initHaptics(){
  //-----------------------------------------------------------------------
  // HAPTIC DEVICES / TOOLS
  //-----------------------------------------------------------------------

  // create a haptic device handler
  handler = new cHapticDeviceHandler();

  // read the number of haptic devices currently connected to the computer
  int numHapticDevices = handler->getNumDevices();

  // if there is at least one haptic device detected...
  if (numHapticDevices)
    {
      // get a handle to the first haptic device
      handler->getDevice(hapticDevice);

      // open connection to haptic device
      hapticDevice->open();

      // initialize haptic device
      hapticDevice->initialize();

      // retrieve information about the current haptic device
      cHapticDeviceInfo info = hapticDevice->getSpecifications();


    }


  h_world = new cWorld;
  h_tool = new cGeneric3dofPointer(h_world);
  h_world->addChild(h_tool);
  // connect the haptic device to the tool
  h_tool->setHapticDevice(hapticDevice);
  // initialize tool by connecting to haptic device
  h_tool->start();
  // map the physical workspace of the haptic device to a larger virtual workspace.
  h_tool->setWorkspaceRadius(WORKSPACE_SCALE);
  // define a radius for the tool
  h_tool->setRadius(tool_size);
  // ***** insert our own force rendering algorithm for the tool *****
  h_force_algorithm = new SimulationForce();
  h_tool->m_proxyPointForceModel = h_force_algorithm; 
  h_tool->m_proxyPointForceModel->initialize(h_world, cVector3d(0,0,0));
  h_tool->m_proxyPointForceModel->setProxyRadius(tool_size);
  h_tool->m_proxyPointForceModel->m_collisionSettings.m_checkBothSidesOfTriangles = false;

  // enable if objects in the scene are going to rotate or translate
  // or possibly collide against the tool. If the environment
  // is entirely static, you can set this parameter to "false"
  h_tool->m_proxyPointForceModel->m_useDynamicProxy = false;
  

  //boundary
  for(int i = 0; i<6; i++){
    wall[i] = new cMesh(h_world);
    wall[i]->newVertex( MAX_X/2*1.1f,MAX_X/2* 1.1f, 0.f);
    wall[i]->newVertex(MAX_X/2* 1.1f,MAX_X/2*-1.1f, 0.f);
    wall[i]->newVertex(MAX_X/2*-1.1f,MAX_X/2*-1.1f, 0.f);
    wall[i]->newVertex(MAX_X/2*-1.1f,MAX_X/2* 1.1f, 0.f);
    wall[i]->newTriangle(0,1,2);
    wall[i]->newTriangle(2,3,0);
    wall[i]->computeBoundaryBox();
    wall[i]->m_material.setStiffness(0.4f);
    h_world->addChild(wall[i]);
  }
  //back wall
  wall[0]->rotate(cVector3d(0.f, 1.f, 0.f), -3.14159265/2.f);
  wall[0]->translate(MAX_X/2*-1.f, 0.f, 0.f);
  //right wall
  wall[1]->rotate(cVector3d(1.f, 0.f, 0.f), -3.14159265/2.f);
  wall[1]->translate(0.f, MAX_X/2*1.f, 0.f);

  //bottom
  wall[2]->rotate(cVector3d(1.f, 0.f, 0.f), 3.14159265);
  wall[2]->translate(0.f, 0.f, MAX_X/2*-1.f);
  //top
  wall[3]->translate(0.f, 0.f, MAX_X/2*1.f);
  //left wall
  wall[4]->rotate(cVector3d(1.f, 0.f, 0.f), 3.14159265/2.f);
  wall[4]->translate(0.f,MAX_X/2*-1.f, 0.f);

  //front, just a tiny little ledge
  wall[5]->rotate(cVector3d(0.f, 1.f, 0.f), 3.14159265/2.f);
  wall[5]->translate(MAX_X/2*1.f, 0.f, -0.f);


  h_vel = make_float3(0.f,0.f,0.f);
  h_force = make_float3(0.f,0.f,0.f);
  force_accumulator.zero();
  vel_accumulator.zero();
}


int main(int argc, char* argv[])
{
  //-----------------------------------------------------------------------
  // INITIALIZATION
  //-----------------------------------------------------------------------

  printf ("\n");
  printf ("-----------------------------------\n");
  printf ("CS277 - Experimental Haptics\n");
  printf ("Homework Boilerplate Application\n");
  printf ("January 2010, Stanford University\n");
  printf ("-----------------------------------\n");
  printf ("\n\n");

  initParams();
  initHaptics();
  initGL(argc, argv);
  
  initCUDA();
  initGraphics();
  

  
  pthread_mutex_init(&cuda_lock,NULL);


  //-----------------------------------------------------------------------
  // START SIMULATION
  //-----------------------------------------------------------------------
  // simulation in now running
  simulationRunning = true;

 
  //graphics clock
  graphics_clock.setTimeoutPeriodSeconds(1.0/30.0f);



  // create a thread which starts the main haptics rendering loop
  cThread* hapticsThread = new cThread();
  hapticsThread->set(updateHaptics, CHAI_THREAD_PRIORITY_HAPTICS);


  cThread* simulationThread = new cThread();
  simulationThread->set(updateSimulation, CHAI_THREAD_PRIORITY_GRAPHICS);



  // start the main graphics rendering loop
  HelpWindow(window_id);
  
  glutMainLoop();

  // close everything
  close();
  // exit
  return (0);
}

//---------------------------------------------------------------------------

void resizeWindow(int w, int h)
{
  // update the size of the viewport
  displayW = w;
  displayH = h;
  glViewport(0, 0, displayW, displayH);
}

//---------------------------------------------------------------------------

void keySelect(unsigned char key, int x, int y)
{
  switch (key){
  case 27: 
  case 'x':
    // close everything
    close();
    // exit application
    exit(0);
    break;
  case 'r':
    resetStuff = true;
    printf("Reset color\n");
    break;
  case 'v':
    resetVel = true;
    printf("Reset to starting velocity\n");
    break;
  case 'b':
    benchmark = !benchmark;
    printf("Bench mark mode %s\n", benchmark?"on":"off");
    break;
  case '1':
    displayMode = DISPLAY_FLUID;
    displayModeText = "Fluid";
    break;
 case '2':
    displayMode = DISPLAY_VELOCITY;
    displayModeText = "Fluid Velocity";
    break;
 case '3':
    displayMode = DISPLAY_PRESSURE;
    displayModeText = "Fluid Pressure";
    break;
 case '4':
    displayMode = DISPLAY_PRESSURE_GRAD;
    displayModeText = "Fluid Pressure Gradient";
    break;
 case '5':
    displayMode = DISPLAY_DIVERGENCE;
    displayModeText = "Fluid Velocity Divergence";
    break;
  case '`':
    displayParam = !displayParam;
    break;
  case ' ':
    resetStuff = true;
    resetVel = true;
    preset = 0;
    break;
  default:
    break;
  }
}

void specialSelect(int key, int x, int y){

  switch (key){
  case GLUT_KEY_F1:
    if(preset==1){
      preset = 0;
    } else {
      preset = 1;
    }
    break;
  case GLUT_KEY_F2:
    if(preset==2){
      preset = 0;
    } else {
      preset = 2;
    }
    break;
  case GLUT_KEY_F3:
    if(preset==3){
      preset = 0;
    } else {
      preset = 3;
    }
    break;
  case GLUT_KEY_F4:
    if(preset==4){
      preset = 0;
    } else {
      preset = 4;
    }
    break;
  case GLUT_KEY_F12:
    preset = 0;
    break;
  default:
    break;
  }
}

//---------------------------------------------------------------------------

void menuSelect(int value)
{
  switch (value)
    {
      // enable full screen display
    case OPTION_FULLSCREEN:
      glutFullScreen();
      break;

      // reshape window to original size
    case OPTION_WINDOWDISPLAY:
      glutReshapeWindow(WINDOW_SIZE_W, WINDOW_SIZE_H);
      break;
    }
}

//---------------------------------------------------------------------------

void close(void)
{
  // stop the simulation
  simulationRunning = false;

  // wait for graphics and haptics loops to terminate
  while (!simulationFinished) { cSleepMs(100); }

  // close the haptic devices
  if (hapticDevice)
    {
      hapticDevice->close();
    }
}

void renderText(){
   glMatrixMode(GL_MODELVIEW);
    glPushMatrix();
    glLoadIdentity();
    glTranslatef(0.0, (GLfloat)(glutGet(GLUT_WINDOW_HEIGHT) - 1.0), 0.0);
    glScalef(1.0, -1.0, 1.0);

    glMatrixMode(GL_PROJECTION);
    glPushMatrix();
    glLoadIdentity();
    glOrtho(0, glutGet(GLUT_WINDOW_WIDTH), 0, glutGet(GLUT_WINDOW_HEIGHT), -1, 1);

    glMatrixMode(GL_MODELVIEW);

    void* font = (void *) GLUT_BITMAP_HELVETICA_18;
    glRasterPos2f((GLfloat)0.f, (GLfloat)displayH-6.f);

    const char *s=displayModeText.c_str();
    int len = (int) strlen(s);
    for (int i = 0; i < len; i++) {
      glutBitmapCharacter(font, s[i]);
    }

    glMatrixMode(GL_PROJECTION);
    glPopMatrix();

    glMatrixMode(GL_MODELVIEW);
    glPopMatrix();

}

//---------------------------------------------------------------------------
//displaydisplay
void updateGraphics(void)
{
  //graphics clock 
  graphics_clock.start(true);
 
  // update the label with the haptic refresh rate
  char buffer[128];
  sprintf(buffer, "haptic rate: %.0lf Hz\tGraphics rate: %f\t Sim rate %f\t", rateEstimate,graphics_rateEstimate, simrateEstimate);

  glutSetWindowTitle(buffer);
  // render world
  glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);  
  glMatrixMode(GL_MODELVIEW);
  glLoadIdentity();


  gluPerspective(60.f, 1.f, 0.1f, float(MAX_X)*1.5f*2);
  
  if(displayParam){
    gluLookAt(0.f, 0.f, float(MAX_X)*1.8f,
	      0.f, 0.f, 0.f,
	      0.f, 1.f, 0.f);
  } else {
    gluLookAt(0.f, 0.f, float(MAX_X)*1.5f,
	      0.f, 0.f, 0.f,
	      0.f, 1.f, 0.f);
  }

 

  glRotatef(cameraPitch, 1.0, 0.0, 0.0);
  glRotatef(cameraYaw, 0.0, 1.0, 0.0);
  
  if(displayParam)
    glTranslatef(0, -MAX_Y/6, 00);

  // cube
  glColor3f(1.0, 1.0, 1.0);
  glutWireCube(float(MAX_X));

  glPushMatrix();
  glTranslatef(h_pos.x, h_pos.y, h_pos.z);
  glColor3f(h_color_r, h_color_g, h_color_b);
  glutSolidSphere(tool_size, 10, 10);
  glPopMatrix();


  //haptic locator

  glLineStipple(3, 0xAAAA);
  glEnable(GL_LINE_STIPPLE);
  glPushMatrix();
  glColor3f(0.0, 0.0, 0.8);
  glBegin(GL_QUADS);
  glVertex3f(h_pos.x+tool_size/2, h_pos.y+tool_size/2, -MAX_X/2);
  glVertex3f(h_pos.x+tool_size/2, h_pos.y-tool_size/2, -MAX_X/2);
  glVertex3f(h_pos.x-tool_size/2, h_pos.y-tool_size/2, -MAX_X/2);
  glVertex3f(h_pos.x-tool_size/2, h_pos.y+tool_size/2, -MAX_X/2);
  glEnd();
  glBegin(GL_LINES);
  glVertex3f(h_pos.x,  h_pos.y, h_pos.z);
  glVertex3f(h_pos.x,  h_pos.y, -MAX_X/2);
  glEnd();
  glPopMatrix();

  glPushMatrix();
  glColor3f(0.0, 0.8, 0.0);
  glBegin(GL_QUADS);
  glVertex3f(h_pos.x+tool_size/2, -MAX_X/2, h_pos.z+tool_size/2);
  glVertex3f(h_pos.x+tool_size/2, -MAX_X/2, h_pos.z-tool_size/2);
  glVertex3f(h_pos.x-tool_size/2, -MAX_X/2, h_pos.z-tool_size/2);
  glVertex3f(h_pos.x-tool_size/2, -MAX_X/2, h_pos.z+tool_size/2);
  glEnd();
  glBegin(GL_LINES);
  glVertex3f(h_pos.x,  h_pos.y, h_pos.z);
  glVertex3f(h_pos.x, -MAX_X/2, h_pos.z);
  glEnd();
  glPopMatrix();


  glPushMatrix();
  glColor3f(0.8, 0.0, 0.0);
  glBegin(GL_QUADS);
  glVertex3f( -MAX_X/2, h_pos.y+tool_size/2, h_pos.z+tool_size/2);
  glVertex3f( -MAX_X/2, h_pos.y+tool_size/2, h_pos.z-tool_size/2);
  glVertex3f( -MAX_X/2, h_pos.y-tool_size/2, h_pos.z-tool_size/2);
  glVertex3f( -MAX_X/2, h_pos.y-tool_size/2, h_pos.z+tool_size/2);
  glEnd();
  glBegin(GL_LINES);
  glVertex3f(h_pos.x,  h_pos.y, h_pos.z);
  glVertex3f(-MAX_X/2, h_pos.y,  h_pos.z);
  glEnd();
  glPopMatrix();
  glDisable(GL_LINE_STIPPLE);

    pthread_mutex_lock(&cuda_lock);
    renderCopy(displayMode);
    pthread_mutex_unlock(&cuda_lock);
  
 if(!benchmark){
   renderFluid();
 }
 
 renderText();
 
 if(displayParam)
   params->Render(0, 0);
  
  // Swap buffers
  glutSwapBuffers();

  // check for any OpenGL errors
  GLenum err;
  err = glGetError();
  if (err != GL_NO_ERROR) printf("Error:  %s\n", gluErrorString(err));

  // inform the GLUT window to call updateGraphics again (next frame)
  if (simulationRunning)
    {
      glutPostRedisplay();
    }
   

  //graphics fps
  while(!graphics_clock.timeoutOccurred());
  graphics_clock.stop();
  graphics_time += graphics_clock.getCurrentTimeSeconds();
  ++graphics_counter;
  if(graphics_counter==100){
    graphics_rateEstimate = float(graphics_counter)/graphics_time;
    graphics_time = 0.f;
    graphics_counter=0;
  }
}

//---------------------------------------------------------------------------

void updateHaptics(void)
{
  // a clock to estimate the haptic simulation loop update rate
  cPrecisionClock pclock;
  pclock.setTimeoutPeriodSeconds(1.0);
  pclock.start(true);
  int counter = 0;


  // main haptic simulation loop
  while(simulationRunning)
    {
      if (!hapticDevice) continue;
		
      // compute global reference frames for each object
      h_world->computeGlobalPositions(true);
      // update position and orientation of tool
      h_tool->updatePose();
      
      // read position of haptic device


      float force_reduction=1.0; //set at 0.04
      float vel_reduction = 0.5;

      cVector3d newPosition = h_tool->getProxyGlobalPos();
      h_pos.x = newPosition.y;
      h_pos.y = newPosition.z;
      h_pos.z = newPosition.x;

      cVector3d linearVelocity= h_tool->m_deviceGlobalVel;
      vel_accumulator= vel_accumulator*vel_decay + (1-vel_decay)*linearVelocity;
      
      h_vel.x = vel_accumulator.y*vel_reduction;
      h_vel.y = vel_accumulator.z*vel_reduction;
      h_vel.z = vel_accumulator.x*vel_reduction;
      
      buttonStatus = 0;
      bool status = false;
      hapticDevice->getUserSwitch(0, status);
      if(status){
	buttonStatus+= 1;
      }
    

      //send force
      cVector3d force;
      force.x = h_force.z*force_reduction/h_mass;
      force.y = h_force.x*force_reduction/h_mass;
      force.z = h_force.y*force_reduction/h_mass;

      force_accumulator = force_accumulator*force_decay+(1-force_decay)*force;   
      h_force_algorithm->AddSimForce(force_accumulator);


      // compute interaction forces
      h_tool->computeInteractionForces();
      h_tool->applyForces();

      // estimate the refresh rate
      ++counter;
      if (pclock.timeoutOccurred()) {
	pclock.stop();
	rateEstimate = counter;
	counter = 0;
	pclock.start(true);
      }
    }
    
  // exit haptics thread
  simulationFinished = true;
}




void updateSimulation(){
 //adaptive delta t clock 
  simulation_clock.setTimeoutPeriodSeconds(1.0);
  simulation_clock.start(true);

  while(!simulation_clock.timeoutOccurred());
  simulation_clock.stop();
  simulation_clock.setTimeoutPeriodSeconds(SIM_SAMPLE_PERIOD);
  simulation_clock.start(true);

  for(int i = 0; i<6; i++){
    wall[i]->m_material.setStiffness(4.f);
  }
  while(simulationRunning){
    pthread_mutex_lock(&cuda_lock);
    stepFluid();
    pthread_mutex_unlock(&cuda_lock);
    
    ++simulation_counter;
    if (simulation_clock.timeoutOccurred()) {
      simulation_clock.stop();
      simrateEstimate = simulation_counter*1/SIM_SAMPLE_PERIOD;
      adaptiveDT= SIM_SAMPLE_PERIOD/float(simulation_counter);
      simulation_counter = 0;
      simulation_clock.start(true);
    }
  }
}



void mouse(int button, int state, int x, int y)
{
  ox = x; oy = y;
  if (state == GLUT_DOWN)
    buttonState |= 1<<button;
  else if (state == GLUT_UP)
    buttonState = 0;

  if (params->Mouse(x, y, button, state)) {
    glutPostRedisplay();
    return;
  }
}


void motion(int x, int y){

  if (params->Motion(x, y)) {
    ox = x; oy = y;
    glutPostRedisplay();
    return;
  }

  float dx, dy;
  dx = (float)(x - ox);
  dy = (float)(y - oy);
  if (buttonState & 1) {
    // left = rotate
    cameraPitch += dy / 5.0f;
    cameraYaw += dx / 5.0f;
  }
  ox = x; oy = y;
}



